Simple Mover: The Code

Below is the complete code for the simple_mover node, in it’s entirety, followed by a step-by-step explanation of what is happening. You can copy and paste this code into the simple_mover script you created in the ~/catkin_ws/src/simple_arm/scripts/ directory like this:

First, open a new terminal, next:

$ cd ~/catkin_ws/src/simple_arm/scripts/
$ nano simple_mover

You have opened the simple_mover script with the nano editor, now copy and paste the code below into the script and use ctrl-x followed by y then enter to save the script.

simple_mover

#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64

def mover():
    pub_j1 = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
                             Float64, queue_size=10)
    pub_j2 = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
                             Float64, queue_size=10)
    rospy.init_node('arm_mover')
    rate = rospy.Rate(10)
    start_time = 0

    while not start_time:
        start_time = rospy.Time.now().to_sec()

    while not rospy.is_shutdown():
        elapsed = rospy.Time.now().to_sec() - start_time
        pub_j1.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        pub_j2.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        rate.sleep()

if __name__ == '__main__':
    try:
        mover()
    except rospy.ROSInterruptException:
        pass

The code: Explained

#!/usr/bin/env python

import math
import rospy

rospy is the official Python client library for ROS. It provides most of the fundamental functionality required for interfacing with ROS via Python. It has interfaces for creating Nodes, interfacing with Topics, Services, Parameters, and more. It will certainly be worth your time to check out the API documentation here . General information about rospy, including other tutorials may be found on the ROS Wiki .

from std_msgs.msg import Float64

From the std_msgs package, we import Float64, which is one of the primitive message types in ROS. The std_msgs package also contains all of the other primitive types. Later on in this script, we will be publishing Float64 messages to the position command topics for each joint.

def mover():
    pub_j1 = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
                             Float64, queue_size= 10)
    pub_j2 = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
                             Float64, queue_size=10)

At the top of the mover function, two publishers are declared, one for joint 1 commands, and one for joint 2 commands. Here, the queue_size parameter is used to determine the maximum number messages that may be stored in the publisher queue before messages are dropped. More information about this parameter can be found here .

 rospy.init_node('arm_mover')

Initializes a client node and registers it with the master. Here “arm_mover” is the name of the node. init_node() must be called before any other rospy package functions are called. The argument anonymous=True makes sure that you always have a unique name for your node

 rate = rospy.Rate(10)

The rate object is created here with a value of 10 Hertz. Rates are used to limit the frequency at which certain loops spin in ROS. Choosing a rate which is too high may result in unnecessarily high CPU usage, while choosing a value too low could result in high overall system latency. Choosing sensible values for all of the nodes in a ROS system is a bit of a fine-art.

    start_time = 0

    while not start_time:
        start_time = rospy.Time.now().to_sec()

start_time is used to determine how much time has elapsed. When using ROS with simulated time (as we are doing here), rospy.Time.now() will initially return 0, until the first message has been received on the /clock topic. This is why start_time is set and polled continuously until a nonzero value is returned (more information here ).

    while not rospy.is_shutdown():
        elapsed = rospy.Time.now().to_sec() - start_time
        pub_j1.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        pub_j2.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        rate.sleep()

This the main loop. Due to the call to rate.sleep() , the loop is traversed at approximately 10 Hertz. Each trip through the body of the loop will result in two joint command messages being published. The joint angles are sampled from a sine wave with a period of 10 seconds, and in magnitude from [ -\pi/2, +\pi/2 ]. When the node receives the signal to shut down (either from the master, or via SIGINT signal in a console window), the loop will be exited.

if __name__ == '__main__':
    try:
        mover()
    except rospy.ROSInterruptException:
        pass

If the name variable is set to “ main ”, indicating that this script is being executed directly, the mover() function will be called. The try/except blocks here are significant as rospy uses exceptions extensively. The particular exception being caught here is the ROSInterruptException . This exception is raised when the node has been signaled for shutdown. If there was perhaps some sort of cleanup needing to be done before the node shuts down, it would be done here. More information about rospy exceptions can be found here .

Running simple_mover

Assuming that your workspace has recently been built, and it’s setup.bash has been sourced, you can launch simple_arm as follows:

$ cd ~/catkin_ws
$ roslaunch simple_arm robot_spawn.launch

Once ROS Master, Gazebo, and all of our relevant nodes are up and running, we can finally launch simple_mover . To do so, open a new terminal and type the following commands:

$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun simple_arm simple_mover

Below is a video showing you what the expected movements should look like.

Congratulations! You’ve now written your first ROS node!